import TUIO.*;

int targetFrameRate = 60;
float tempFrameRate = 0;
int counter = 0;

TuioProcessing tuioClient;
ArrayList<Block> templates;
ArrayList<Block> incomplete;
ArrayList<Block> complete;
HashMap<Long, Block> selected; // selected (touched) blocks
HashMap<Long, PVector> selectedLastPos; // last coordinates of selected blocks
HashSet<Long> cursorIds; // all tracked touches
HashSet<Long> ignoreIds; // touches that were used to complete block are ignored

ArrayList<PVector> completeEffect; // visual effect used when block is completed

void setup() {
  size(screen.width - 100, screen.height - 100);
  frameRate(targetFrameRate);
  background(255);
  noFill();
  tuioClient = new TuioProcessing(this);
  
  templates = new ArrayList<Block>();
  incomplete = new ArrayList<Block>();
  complete = new ArrayList<Block>();
  selected = new HashMap<Long, Block>();
  selectedLastPos = new HashMap<Long, PVector>();
  cursorIds = new HashSet<Long>();
  ignoreIds = new HashSet<Long>();
  
  completeEffect = new ArrayList<PVector>();
  
  Block template = new BlockX();
  template.add(20, 20);
  template.add(80, 20);
  template.add(80, 120);
  template.add(20, 120);
  templates.add(template);
  template = new BlockX();
  template.add(180, 20);
  template.add(220, 120);
  template.add(140, 120);
  templates.add(template);
  template = new BlockX();
  template.add(280, 20);
  template.add(340, 30);
  template.add(340, 70);
  template.add(280, 120);
  template.add(280, 80);
  templates.add(template);
  String[] params = { "C:/AaltoWindow/frameworks/java/processing/TuioOverlay.exe", "P5", frame.getTitle() }; open(params);
}

void draw() {
  // handle cursor removal
  Iterator iter = cursorIds.iterator();
  while (iter.hasNext()) {
    long id = (Long)iter.next();
    if (tuioClient.getTuioCursor(id) == null) { // cursor has disappeared (touch has ended)
      for (Block b : incomplete) {
        if (b.remove(id)) { // remove cursor from incomplete block
          if (b.size() == 0) { // remove block if it does not have any vertices
            incomplete.remove(b);
          }
          break;
        }
      }
      Block s = selected.remove(id);
      if (s != null) { // remove block selection
        s.setVelocity(); // set velocity for the block
        selectedLastPos.remove(id);
      }
      ignoreIds.remove(id); // remove cursor from ignored cursors
      iter.remove(); // remove cursor from all tracked cursors
    }
  }
  
  // handle cursors
  Vector tuioCursorList = tuioClient.getTuioCursors();
  for (int i = 0; i < tuioCursorList.size(); ++i) {
    TuioCursor tcur = (TuioCursor)tuioCursorList.elementAt(i);
    long id = tcur.getSessionID();
    if (ignoreIds.contains(id)) continue; // ignore cursors previously used to complete block
    
    float x = tcur.getScreenX(width);
    float y = tcur.getScreenY(height);
    
    // move vertex belonging to incomplete block
    boolean isContained = false;
    for (Block b : incomplete) {
      if (b.contains(id)) { // cursor belongs to incomplete block
        if (b.near(x, y)) { // update cursor position if it is near block
          b.update(x, y, id);
          isContained = true;
        }
        else {
          b.remove(id); // separate cursor from block if it is too far from block
        }
        break;
      }
    }
    if (isContained) continue;
    
    // or move selected block
    Block s = selected.get(id);
    if (s != null) { // cursor is used to select complete block
      PVector lastPos = selectedLastPos.get(id);
      float dx = x - lastPos.x;
      float dy = y - lastPos.y;
      s.translate(dx, dy);
      s.addVelocityBuffer(dx, dy); // buffer current velocity of block
      lastPos.set(x, y, 0); // store current position of block
      continue;
    }
    
    // or start new selection
    boolean isSelected = false;
    for (Block b : complete) {
      if (b.insideHull(x, y)) { // cursor is inside complete block
        selected.put(id, b); // register newly selected block
        selectedLastPos.put(id, new PVector(x, y)); // and its current position
        b.resetVelocity(); // set velocity of block to zero and clear its velocity buffer
        cursorIds.add(id); // store new touch
        isSelected = true;
        break;
      }
    }
    if (isSelected) continue;
    
    // or otherwise add to block nearby
    float nearestD = Float.POSITIVE_INFINITY;
    Block nearestB = null;
    for (Block b : incomplete) {
      float d = b.dist2(x, y);
      if (d < nearestD) { // find nearest incomplete block to cursor
        nearestD = d;
        nearestB = b;
      }
    }
    if (nearestB != null && nearestB.near(x, y)) { // if cursor is near enough to nearest block
      nearestB.add(x, y, id); // add cursor to block
    }
    else { // if cursor is too far from all blocks create new incomplete block
      BlockX b = new BlockX();
      b.add(x, y, id);
      incomplete.add(b);
    }
    cursorIds.add(id); // store new touch
  }
  
  // merge incomplete blocks if they are near each other
  boolean isMerged = true;
  while (isMerged) {
    isMerged = false;
    for (int i = 0; i < incomplete.size() - 1; ++i) {
      for (int j = i + 1; j < incomplete.size(); ++j) {
        Block b1 = incomplete.get(i);
        Block b2 = incomplete.get(j);
        if (b1.near(b2)) { // blocks are near each other
          b1.merge(b2); // merge them
          incomplete.remove(j); // remove other block
          isMerged = true;
          break;
        }
      }
      if (isMerged) break;
    }
  }
  
  background(255);
  
  // if incomplete block is similar to template block, complete it
  for (Block template : templates) {
    iter = incomplete.iterator();
    while (iter.hasNext()) {
      Block b = (Block)iter.next();
      if (b.similar(template)) { // incomplete block is similar to template block
        complete.add(b); // add to complete
        iter.remove(); // remove from incomplete
        for (PVector v : b.vertices) {
          long id = (long)v.z;
          ignoreIds.add(id); // ignore cursors used to complete block
        }
        completeEffect.add(new PVector(b.centroid.x, b.centroid.y, 10)); // visual effect when block is completed
      }
    }
    template.draw();
  }

  // bounce off walls
  for (Block b : complete) {
    if (b.velocity != null) {
      if (b.centroid.x < 0) {
        b.translate(0 - b.centroid.x, 0); // prevent block to get trapped inside wall due to lack of velocity
        b.velocity.x *= -1; // invert velocity component
      }
      if (b.centroid.x > width) {
        b.translate(width - b.centroid.x, 0);
        b.velocity.x *= -1;
      }
      if (b.centroid.y < 0) {
        b.translate(0, 0 - b.centroid.y);
        b.velocity.y *= -1;
      }
      if (b.centroid.y > height) {
        b.translate(0, height - b.centroid.y);
        b.velocity.y *= -1;
      }
    }
    b.go(); // move block according to its velocity
    if (selected.containsValue(b)) { // color selected block
      fill(96, 96, 255);
    }
    else {
      fill(128, 128, 128);
    }
    b.draw();
  }
  
  stroke(0);
  noFill();
  for (Block b : incomplete) { // draw incomplete
    b.draw();
  }
  
  pushStyle();
  stroke(0, 0, 255);
  iter = completeEffect.iterator();
  while (iter.hasNext()) {
    PVector v = (PVector)iter.next();
    strokeWeight(200 / v.z);
    stroke(255, 127, 0, 255 - v.z - 55);
    ellipse(v.x, v.y, v.z, v.z);  // draw visual effect
    v.z += 5;
    if (v.z > 200) iter.remove(); // visual effect ends
  }
  popStyle();
  
  pushStyle();
  fill(0);
  if (++counter > targetFrameRate) {
    tempFrameRate = frameRate;
    counter = 0;
  }
  text("FPS: " + String.format("%.2f", tempFrameRate), 400, 20); // draw FPS
  popStyle();
  
  iter = incomplete.iterator();
  while (iter.hasNext()) if (((Block)iter.next()).size() == 0) iter.remove(); // remove empty incomplete blocks that do not get removed for some reason
}

// called when a cursor is added to the scene
void addTuioCursor(TuioCursor tcur) {
  println(tcur.getSessionID() + " added");
}

// called when a cursor is moved
void updateTuioCursor (TuioCursor tcur) {
}

// called when a cursor is removed from the scene
void removeTuioCursor(TuioCursor tcur) {
  println(tcur.getSessionID() + " removed");
}

